package GameObjects.Physics;

public class Position 
{
	public static final double FRICTION = 0.95;
	
	private double x;
	public double x(){return x;}
	private double y;
	public double y(){return y;}
	private double theta;
	public double theta(){return theta;}
	
	private double vx;
	private double vy;
	
	public Position(double x, double y, double theta)
	{
		this.x = x;
		this.y = y;
		this.theta = theta;
	}
	
	public Position(Position position)
	{
		this.x = position.x;
		this.y = position.y;
		this.theta = position.theta;
	}
	
	public void moveTo(double dx, double dy, double theta)
	{
		this.x = dx;
		this.y = dy;
		this.theta = theta;
	}
	
	public DeltaPosition translate(double dx, double dy)
	{
		this.x += dx;
		this.y += dy;
		return new DeltaPosition(dx, dy, 0);
	}
	
	public DeltaPosition translateAnlge(double dTheta)
	{
		addAngle(dTheta);
		return new DeltaPosition(0, 0, dTheta);
	}
	
	public DeltaPosition moveTowards(Position target, double speed, double rotateSpeed)
	{
		return new DeltaPosition(0, 0, 0);
	}
	
	public void addFriction()
	{
		this.vx *= FRICTION;
		if(this.vx < 0.1 && this.vx > -0.1)
			this.vx = 0;
		this.vy *= FRICTION;
		if(this.vy < 0.1 && this.vy > -0.1)
			this.vy = 0;
	}
	
	public void stop()
	{
		this.vx = 0;
		this.vy = 0;
	}

	public DeltaPosition moveForwardConserveMomentum(double speed)
	{
		double dTheta = 0;
		if(speed > 0 || speed < 0)
		{
			double radians = theta * Math.PI/180;
			this.vx += Math.cos(radians) * speed;
			this.vy += Math.sin(radians) * speed;
		}
		else
			addFriction();
		x += this.vx;
		y += this.vy;
		return new DeltaPosition(this.vx, this.vy, dTheta);
	}
	
	public DeltaPosition moveForward(double speed)
	{
		double radians = theta * Math.PI/180;
		this.vx = Math.cos(radians) * speed;
		x += this.vx;
		this.vy = Math.sin(radians) * speed;
		y += this.vy;
		double dTheta = 0;	
		return new DeltaPosition(this.vx, this.vy, dTheta);
	}
	
	public DeltaPosition turn(double rotateSpeed)
	{
		double dTheta = addAngle(rotateSpeed);
		return new DeltaPosition(0, 0, dTheta);
	}
	
	public double addAngle(double dTheta)
	{
		double oTheta = theta;
		theta += dTheta;
		if(theta >= 360)
			theta -= 360;
		else if(theta < 0)
			theta += 360;
		return theta - oTheta;
	}
	
	public DeltaPosition rotateAround(Position aroundPosition, DeltaPosition dPosition)
	{
		double cx = aroundPosition.x - x;
		double cy = aroundPosition.y - y;
		
		double radians = -dPosition.dTheta * Math.PI/180;
	
		this.x = cx * Math.cos(radians) + cy * Math.sin(radians);
	    this.y = cy * Math.cos(radians) - cx * Math.sin(radians);
	
		this.theta += dPosition.dTheta;
		
		return dPosition;
	}
}
